#include "lidar_localization/subscriber/cloud_subscriber.hpp"
#include "glog/logging.h"
#include <pcl_conversions/pcl_conversions.h>  // 添加这一行

namespace lidar_localization {
    // 构造函数接受一个 ROS 节点句柄 nh、一个话题名称 topic_name 和一个缓冲区大小 buff_size。
CloudSubscriber::CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size)
    : nh_(nh) {
    // 使用 nh_.subscribe 方法订阅指定的话题，并指定回调函数 msg_callback，当接收到消息时会调用该函数。
    subscriber_ = nh_.subscribe(topic_name, buff_size, &CloudSubscriber::msg_callback, this);
}

void CloudSubscriber::msg_callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg_ptr) {
    buff_mutex_.lock();
    // 使用互斥锁 buff_mutex_ 来保护对共享数据的访问，确保线程安全。

    CloudData cloud_data;
    // 创建一个 CloudData 对象，并将激光点云消息的时间戳和点云数据存储到 cloud_data 中。
    
    cloud_data.time = cloud_msg_ptr->header.stamp.toSec();
    // 获取传感器数据的时间戳，并将其以秒为单位存储到 cloud_data.time 中。cloud_msg_ptr->header.stamp 是 ROS 消息头中的时间戳

    pcl::fromROSMsg(*cloud_msg_ptr, *(cloud_data.cloud_ptr));
    // 使用 pcl::fromROSMsg 将 ROS 消息转换为 PCL（Point Cloud Library）格式的点云数据。

    new_cloud_data_.push_back(cloud_data);
    // 将新的点云数据添加到 new_cloud_data_ 队列中。
    buff_mutex_.unlock();
}

void CloudSubscriber::ParseData(std::deque<CloudData>& cloud_data_buff) {
    // ParseData 函数用于将新接收到的点云数据解析并传递到外部缓冲区 cloud_data_buff。
    buff_mutex_.lock();
    // 同样使用互斥锁确保线程安全。

    if (new_cloud_data_.size() > 0) {
        cloud_data_buff.insert(cloud_data_buff.end(), new_cloud_data_.begin(), new_cloud_data_.end());
        new_cloud_data_.clear();
    }
    // 如果 new_cloud_data_ 中有新数据，将其添加到 cloud_data_buff 中，并清空 new_cloud_data_。
    buff_mutex_.unlock();
}
} 